#!/usr/bin/env python3

from concurrent.futures import Future
import rclpy
from rclpy.node import Node

from rclpy.action import ActionClient, GoalResponse, CancelResponse
from rclpy.action.client import ClientGoalHandle, GoalStatus, CancelGoal
from ros2_py_pkg.action import MoveRobot
from example_interfaces.msg import Empty


class MoveRobotActionClient(Node):
    def __init__(self):
        super().__init__("move_robot_action_client")

        self.goal_handle_: ClientGoalHandle = None

        self.move_robot_action_client_ = ActionClient(
            self, MoveRobot, "move_robot")

        self.sub_ = self.create_subscription(
            Empty, "move_robot_cancel", self.move_robot_cancel_callback, 10)
        
        self.get_logger().info("Action client has been started")
        
    def send_goal(self, position, velocity):
        self.move_robot_action_client_.wait_for_server()
        
        goal = MoveRobot.Goal()
        goal.position = position
        goal.velocity = velocity

        self.get_logger(). \
            info("Send goal with position " + str(position) + " and velocity " + str(velocity))
        self.move_robot_action_client_. \
            send_goal_async(goal, self.goal_feedback). \
            add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future: Future):
        self.goal_handle_ = future.result()

        if self.goal_handle_.accepted:
            self.get_logger().info("Goal got accepted")
            self.goal_handle_.get_result_async().add_done_callback(self.goal_result_callback)
        else:
            self.get_logger().info("Goal got rejected")

    def goal_result_callback(self, future: Future):
        status = future.result().status
        result: MoveRobot.Result = future.result().result

        if status == GoalStatus.STATUS_SUCCEEDED:
            self.get_logger().info("Succeeded")
        elif status == GoalStatus.STATUS_ABORTED:
            self.get_logger().warn("Aborted")
        elif status == GoalStatus.STATUS_CANCELED:
            self.get_logger().warn("Canceled")

        self.get_logger().info("Position: " + str(result.position))
        self.get_logger().info("Message: " + str(result.message))

    def goal_feedback(self, feedback_msg):
        position = feedback_msg.feedback.current_position
        self.get_logger().info("Feedback position: " + str(position))

    def move_robot_cancel_callback(self, msg):
        self.cancel_goal()

    def cancel_goal(self):
        if self.goal_handle_ is not None:
            self.get_logger().info("Send a cancel request")
            self.goal_handle_.cancel_goal_async()
            
            

def main(argc=None):
    rclpy.init()
    move_robot_action_client = MoveRobotActionClient()

    try:
        move_robot_action_client.send_goal(70,1)
        rclpy.spin(move_robot_action_client)
        rclpy.shutdown()
    except KeyboardInterrupt:
        print("Terminating Node...")
        move_robot_action_client.move_robot_action_client_.destroy()
        move_robot_action_client.destroy_node()

if __name__ == "__main__":
    main()